#include "System.h"
#include <thread>
#include <pangolin/pangolin.h>
#include "nav_msgs/Odometry.h"

using namespace std;

namespace msf
{

/**
 * @brief 系统的构造函数，将会启动其他的线程
 * @param sensor 传感器类型
 * @param bUseViewer 是否使用可视化界面
 * @param initFr initFr表示初始化帧的id,开始设置为0
 * @param strSequence 序列名,在跟踪线程和局部优化线程用得到
 */
System::System(const string &strSettingsFile, const eSensor sensor, const bool bUseViewer, const int initFr): 
      mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false), 
      mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)
{     

      cout << "Input sensor was set to: ";

      if(mSensor==IMU_ODOM)
      {
            cout << "Imu+Odometry" << endl;      //轮速计 + imu
      }

      //读取配置文件
      settings_ = new Settings(strSettingsFile, mSensor);

      // 多地图管理，这里加载Atlas标识符
      bool loadedAtlas = false;

      //Create the Atlas
      //创建多地图，参数0表示初始化关键帧id为0
      cout << "System : Initialization of Atlas from scratch " << endl;
      mpAtlas = new Atlas(0);

      //如果是有imu的传感器类型，设置mbIsInertial = true;以后的跟踪和预积分将和这个标志有关
      if (mSensor==IMU_ODOM)
            mpAtlas->SetInertialSensor();

      // 依次创建跟踪、局部优化和显示线程
      // 创建用于显示位姿帧的类，由Viewer调用
      mpMapDrawer = new MapDrawer(mpAtlas, settings_);

      // 初始化Tracking线程：主线程，不会立刻开启，在对图像和imu预处理后在main主线程中执行
      mpTracker = new Tracking(this, mpMapDrawer, mpAtlas, mSensor, settings_);

      // 初始化局部优化线程并启动
      mpLocaloptimizer = new LocalOptimizing(this, mpAtlas, mSensor==IMU_ODOM);
      mpLocaloptimizing = new thread(&msf::LocalOptimizing::Run, mpLocaloptimizer);
      mpLocaloptimizer->mInitFr = initFr;

      // 设置线程间的指针
      mpTracker->SetLocalOptimizer(mpLocaloptimizer);
      
      mpLocaloptimizer->SetTracker(mpTracker);
      
      // 创建并开启显示线程
      if(bUseViewer)
      {
            mpViewer = new Viewer(this, mpMapDrawer, mpTracker, settings_);
            mptViewer = new thread(&Viewer::Run, mpViewer);
            mpTracker->SetViewer(mpViewer);
            //mpViewer->both = mpMapDrawer->both;
      }

}

Sophus::SE3f System::Track(const nav_msgs::OdometryConstPtr &odom, const double &timestamp, const vector<IMU::Point>& vImuMeas)
{
      if(mSensor!=IMU_ODOM)
      {
            cerr << "ERROR: you called Track but input sensor was not set to IMU_ODOM." << endl;
            exit(-1);
      }

      // 检查模式切换
      {
            // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
            unique_lock<mutex> lock(mMutexMode);
            // mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式
            if(mbActivateLocalizationMode)
            {
                  // Problem7.2 mbonlytracking时关键帧一直没有插入地图，因为局部地图线程被关闭了
                  //mpLocaloptimizer->RequestStop();

                  // Wait until Local Mapping has effectively stopped
                  //while(!mpLocaloptimizer->isStopped())
                  //{
                        //usleep(1000);
                  //}
                  // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
                  mpTracker->InformOnlyTracking(true);
                  mbActivateLocalizationMode = false;
            }
            if(mbDeactivateLocalizationMode)
            {
                  mpTracker->InformOnlyTracking(false);
                  mpLocaloptimizer->Release();
                  mbDeactivateLocalizationMode = false;
            }            
      }

      // 检查reset
      {
            unique_lock<mutex> lock(mMutexReset);
            if(mbReset)
            {
                  mpTracker->Reset();
                  mbReset = false;
            }
            else if(mbResetActiveMap)
            {
                  mpTracker->ResetActiveMap();
                  mbResetActiveMap = false;
            }
      }

      // 把IMU数据存储到队列mlQueueImuData
      if(mSensor == System::IMU_ODOM)
            for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
                  mpTracker->GrabImuData(vImuMeas[i_imu]);
      
      // 计算odom位姿
      Sophus::SE3f Tow = mpTracker->GrabOdom(odom, timestamp);

      // 更新跟踪状态和参数
      unique_lock<mutex> lock2(mMutexState);
      mTrackingState = mpTracker->mState;

      return Tow;
}


void System::ActivateLocalizationMode()
{
    unique_lock<mutex> lock(mMutexMode);
    mbActivateLocalizationMode = true;
}

void System::DeactivateLocalizationMode()
{
    unique_lock<mutex> lock(mMutexMode);
    mbDeactivateLocalizationMode = true;
}

void System::Reset()
{
      unique_lock<mutex> lock(mMutexReset);
      mbReset = true;
}

void System::ResetActiveMap()
{
      unique_lock<mutex> lock(mMutexReset);
      mbResetActiveMap = true;
}

void System::Shutdown()
{
      {
            unique_lock<mutex> lock(mMutexReset);
            mbShutDown = true;
      }

      cout << "Shutdown" << endl;

      if (mpViewer)
      {
            mpViewer->RequestFinish();
            while (!mpViewer->isFinished())
                  usleep(5000);
      }

      mpLocaloptimizer->RequestFinish();
      while(!mpLocaloptimizer->isFinished())
      {
            usleep(5000);
      }

}

bool System::isShutDown()
{
      unique_lock<mutex> lock(mMutexReset);
      return mbShutDown;
}

void System::SaveTrajectoryEuRoC(const string &filename)
{
      cout << endl << "Saving trajectory to " << filename << " ..." << endl;

      vector<Map*> vpMaps = mpAtlas->GetAllMaps();
      int numMaxKPFs = 0;
      Map* pBiggerMap;
      std::cout << "There are " << std::to_string(vpMaps.size()) << " maps in the atlas" << std::endl;
      for(Map* pMap :vpMaps)
      {
            std::cout << "  Map " << std::to_string(pMap->GetId()) << " has " << std::to_string(pMap->GetAllKeyPoseFrames().size()) << " KPFs" << std::endl;
            if(pMap->GetAllKeyPoseFrames().size() > numMaxKPFs)
            {
                  numMaxKPFs = pMap->GetAllKeyPoseFrames().size();
                  pBiggerMap = pMap;
            }
      }

      vector<KeyPoseFrame*> vpKPFs = pBiggerMap->GetAllKeyPoseFrames();
      sort(vpKPFs.begin(),vpKPFs.end(),KeyPoseFrame::lId);

      // Transform all keyposeframes so that the first keyposeframe is at the origin.
      // After a loop closure the first keyposeframe might not be at the origin.
      Sophus::SE3f Twb; // Can be world to odom or world to b depending on IMU or not.
      if (mSensor==IMU_ODOM)
            Twb = vpKPFs[0]->GetImuPose();
      else
            Twb = vpKPFs[0]->GetPoseInverse();

      ofstream f;
      f.open(filename.c_str());
      // cout << "file open" << endl;
      f << fixed;

      // PoseFrame pose is stored relative to its reference keyposeframe (which is optimized by BA and pose graph).
      // We need to get first the keyposeframe pose and then concatenate the relative transformation.
      // PoseFrames not localized (tracking failure) are not saved.

      // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
      // which is true when tracking failed (lbL).
      list<msf::KeyPoseFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
      list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
      list<bool>::iterator lbL = mpTracker->mlbLost.begin();

      for(auto lit=mpTracker->mlRelativeFramePoses.begin(),
            lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
      {
            if(*lbL)
                  continue;

            KeyPoseFrame* pKPF = *lRit;

            Sophus::SE3f Trw;

            if (!pKPF)
                  continue;

            while(pKPF->isBad())
            {
                  Trw = Trw * pKPF->mTop;
                  pKPF = pKPF->GetParent();
            }            

            if(!pKPF || pKPF->GetMap() != pBiggerMap)
            {
                  continue;
            }

            Trw = Trw * pKPF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference

            if (mSensor == IMU_ODOM)
            {
                  Sophus::SE3f Twb = (pKPF->mImuCalib.mTbo * (*lit) * Trw).inverse();
                  Eigen::Quaternionf q = Twb.unit_quaternion();
                  Eigen::Vector3f twb = Twb.translation();
                  f << setprecision(6) << 1e9*(*lT) << " " <<  setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
            }
            else
            {
                  Sophus::SE3f Two = ((*lit)*Trw).inverse();
                  Eigen::Quaternionf q = Two.unit_quaternion();
                  Eigen::Vector3f two = Two.translation();
                  f << setprecision(6) << 1e9*(*lT) << " " <<  setprecision(9) << two(0) << " " << two(1) << " " << two(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
            }
      }

      f.close();
      cout << endl << "End of saving trajectory to " << filename << " ..." << endl;
}

void System::SaveKeyPoseFrameTrajectoryEuRoC(const string &filename)
{
      cout << endl << "Saving keyposeframe trajectory to " << filename << " ..." << endl;

      vector<Map*> vpMaps = mpAtlas->GetAllMaps();
      Map* pBiggerMap;
      int numMaxKPFs = 0;
      for(Map* pMap :vpMaps)
      {
            if(pMap && pMap->GetAllKeyPoseFrames().size() > numMaxKPFs)
            {
                  numMaxKPFs = pMap->GetAllKeyPoseFrames().size();
                  pBiggerMap = pMap;
            }
      }

      if(!pBiggerMap)
      {
            std::cout << "There is not a map!!" << std::endl;
            return;
      }

      vector<KeyPoseFrame*> vpKPFs = pBiggerMap->GetAllKeyPoseFrames();
      sort(vpKPFs.begin(),vpKPFs.end(),KeyPoseFrame::lId);

      ofstream f;
      f.open(filename.c_str());
      f << fixed;

      for(size_t i=0; i<vpKPFs.size(); i++)
      {
            KeyPoseFrame* pKPF = vpKPFs[i];
            if(!pKPF || pKPF->isBad())
                  continue;
            if (mSensor == IMU_ODOM)
            {
                  Sophus::SE3f Twb = pKPF->GetImuPose();
                  Eigen::Quaternionf q = Twb.unit_quaternion();
                  Eigen::Vector3f twb = Twb.translation();
                  f << setprecision(6) << 1e9*pKPF->mTimeStamp  << " " <<  setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
            }
            else
            {
                  Sophus::SE3f Two = pKPF->GetPoseInverse();
                  Eigen::Quaternionf q = Two.unit_quaternion();
                  Eigen::Vector3f t = Two.translation();
                  f << setprecision(6) << 1e9*pKPF->mTimeStamp << " " <<  setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
            }
      }
      f.close();
}

int System::GetTrackingState()
{
      unique_lock<mutex> lock(mMutexState);
      return mTrackingState;
}



} // namespace msf
